#include "motor.h"
#include "ti_msp_dl_config.h"

#define PWM_PERIOD_COUNT (1000)

#define get_pos_index(a, b) ((int)(a | b))

volatile static int32_t encoder_cnt[4] = {0};

void motor_init(void)
{
  NVIC_EnableIRQ(ENC_INT_IRQN);

  DL_Timer_startCounter(MOTOR_PWM_R_INST);
  DL_Timer_startCounter(MOTOR_PWM_F_INST);
  for (int i = 0; i < 4; i++)
    encoder_cnt[i] = 0;
}
void motor_set_speed(motor_position_t pos, int16_t speed)
{
  int16_t duty = PWM_PERIOD_COUNT - speed;
  GPTIMER_Regs *set_pwm_timer =
      (pos & MOTOR_FRONT) ? MOTOR_PWM_F_INST : MOTOR_PWM_R_INST;
  DL_TIMER_CC_INDEX set_channel =
      (pos & MOTOR_LEFT) ? DL_TIMER_CC_0_INDEX : DL_TIMER_CC_1_INDEX;

  if (duty > PWM_PERIOD_COUNT)
    duty = PWM_PERIOD_COUNT;
  else if (duty < 0)
    duty = 0;

  DL_TimerG_setCaptureCompareValue(set_pwm_timer, duty, set_channel);
}
void motor_set_mode(motor_position_t pos, motor_mode_t mode)
{

  uint32_t in1 = 0;
  uint32_t in2 = 0;

  if (pos & MOTOR_FRONT)
  {
    if (pos & MOTOR_LEFT)
    {
      in1 = MOTOR_FL_IN1_PIN;
      in2 = MOTOR_FL_IN2_PIN;
    }
    else
    {
      in1 = MOTOR_FR_IN1_PIN;
      in2 = MOTOR_FR_IN2_PIN;
    }
  }
  else
  {
    if (pos & MOTOR_LEFT)
    {
      in1 = MOTOR_RL_IN1_PIN;
      in2 = MOTOR_RL_IN2_PIN;
    }
    else
    {
      in1 = MOTOR_RR_IN1_PIN;
      in2 = MOTOR_RR_IN2_PIN;
    }
  }

  /*
              in1     in2
  forward     1       0
  backward    0       1
  stop        1       1
  neutral     0       0
  */
  switch (mode)
  {
  default:
  case MOTOR_NEUTRAL:
    DL_GPIO_clearPins(MOTOR_PORT, in1);
    DL_GPIO_clearPins(MOTOR_PORT, in2);
    break;
  case MOTOR_FORWARD:
    DL_GPIO_setPins(MOTOR_PORT, in1);
    DL_GPIO_clearPins(MOTOR_PORT, in2);
    break;
  case MOTOR_BACKWARD:
    DL_GPIO_clearPins(MOTOR_PORT, in1);
    DL_GPIO_setPins(MOTOR_PORT, in2);
    break;
  case MOTOR_STOP:
    DL_GPIO_setPins(MOTOR_PORT, in1);
    DL_GPIO_setPins(MOTOR_PORT, in2);
    break;
  }
}

int32_t motor_get_encoder_cnt(motor_position_t pos)
{
  return encoder_cnt[(int)pos];
}
void GROUP1_IRQHandler(void)
{
  // 读取触发的 GPIOA 和 GPIOB 中断状态
  uint32_t PortA_interrup = DL_GPIO_getRawInterruptStatus(
      ENC_RL_OUT_A_PORT, ENC_RL_OUT_A_PIN | ENC_RR_OUT_A_PIN);

  if (PortA_interrup & ENC_RL_OUT_A_PIN)
  { // rising interrupt
    if (DL_GPIO_readPins(ENC_RL_OUT_B_PORT, ENC_RL_OUT_B_PIN) &
        ENC_RL_OUT_B_PIN)
      encoder_cnt[get_pos_index(MOTOR_REAR, MOTOR_LEFT)]--;
    else
      encoder_cnt[get_pos_index(MOTOR_REAR, MOTOR_LEFT)]++;
  }

  if (PortA_interrup & ENC_RR_OUT_A_PIN)
  {
    if (DL_GPIO_readPins(ENC_RR_OUT_B_PORT, ENC_RR_OUT_B_PIN) &
        ENC_RR_OUT_B_PIN)
      encoder_cnt[get_pos_index(MOTOR_REAR, MOTOR_RIGHT)]++;
    else
      encoder_cnt[get_pos_index(MOTOR_REAR, MOTOR_RIGHT)]--;
  }

  DL_GPIO_clearInterruptStatus(ENC_RL_OUT_A_PORT,
                               PortA_interrup &
                                   (ENC_RL_OUT_A_PIN | ENC_RR_OUT_A_PIN));
}
